﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Collections.ObjectModel;
using System.Windows.Media.Media3D;

namespace FeatureDetection {
    public static class FeatureDetector {
        #region defines
        public class Trajection_Part {
            public Trajection_Part(int num, double x, double y, double z = 0) {
                this._num = num;
                this._x = x;
                this._y = y;
                this._z = z;
            }
            public Trajection_Part(int num, Point3D point){
                this._num = num;
                this._x = point.X;
                this._y = point.Y;
                this._z = point.Z;
            }
            public Trajection_Part(int num, Vector3D vec) {
                this._num = num;
                this._x = vec.X;
                this._y = vec.Y;
                this._z = vec.Z;
            }
            public Trajection_Part(int num, Point pos) : this(num, pos.X, pos.Y) { }

            private int _num;
            public int Num {
                get { return _num; }
                set { _num = value; }
            }

            private double _x;
            public double X {
                get { return _x; }
                set { _x = value; }
            }

            private double _y;
            public double Y {
                get { return _y; }
                set { _y = value; }
            }

            private double _z;
            public double Z {
                get { return _z; }
                set { _z = value; }
            }

            public Point ToPoint() {
                return new Point(this.X, this.Y);
            }
        }
        #endregion
        public static ICollection<Trajection_Part> TrajectionToFeature(ICollection<Trajection_Part> trajection) {
            var ret = new Collection<Trajection_Part>();

            //重心からの角座標にした時の、角度の密度
            double x=0, y=0, z=0;
            foreach(var pos in trajection) {
                x += pos.X;
                y += pos.Y;
                z += pos.Z;
            }
            x /= trajection.Count;
            y /= trajection.Count;
            z /= trajection.Count;

            Point3D g = new Point3D(x, y, z);   //重心

            const int div = 36;

            int[] argx = new int[div];
            int[] argy = new int[div];
            int[] argz = new int[div];
            foreach(var pos in trajection) {
                Point3D p = new Point3D(pos.X, pos.Y, pos.Z);
                Vector3D v = p - g;
                ++argx[(int)Math.Floor((Math.Atan2(-v.Z, v.Y) / (2 * Math.PI) + 1) * div % div)];
                ++argy[(int)Math.Floor((Math.Atan2(v.X, -v.Z) / (2 * Math.PI) + 1) * div % div)];
                ++argz[(int)Math.Floor((Math.Atan2(v.X, v.Y) / (2 * Math.PI) + 1) * div % div)];
            }

            for(int i = 0; i < div; ++i) ret.Add(new Trajection_Part(1, i, argx[i]));
            for(int i = 0; i < div; ++i) ret.Add(new Trajection_Part(2, i, argy[i]));
            for(int i = 0; i < div; ++i) ret.Add(new Trajection_Part(3, i, argz[i]));
            /*
            //角度の変化量
            var vec = new List<Point3D>();
            foreach(var item in trajection) {
                vec.Add(new Point3D(item.X, item.Y, item.Z));
            }

            for(int i = 1; i+1 < vec.Count; ++i) {
                Vector3D ba = vec[i] - vec[i-1];
                Vector3D cb = vec[i + 1] - vec[i];
                float n1 = (float)(ba.X*cb.X+ba.Y*cb.Y+ba.Z*cb.Z);
                float n2 = (float)(ba.Length * cb.Length);
                double theta = Math.Acos(n1/n2);
                ret.Add(new Trajection_Part(4, i, theta));
            }*/

            return ret;
        }
    }
}
